function [fun, grad] = PL_penalty_parallel(beta, varargin)
% [fun, grad] = PL_penalty_parallel(beta, varargin)
% 
% PL_penalty_parallel computes the functional for Parallel Level Sets [1]
% and its gradient.
%
% See also: PL_penalty_CTV, PL_penalty_Nambu, PL_penalty
%
% [1] M.J. Ehrhardt and S. Arridge, 
%     Vector-Valued Image Processing by Parallel Level Sets, 
%     IEEE Transactions on Image Processing (to appear), 2013
%
% 2013-07-25 --------------------------------------------------------------
% 
% Matthias J. Ehrhardt
% CMIC, University College London, UK
% 
% matthias.ehrhardt.11@ucl.ac.uk
% 
% -------------------------------------------------------------------------
    
    if iscell(varargin{1})
        K = length(varargin{1});
        input = varargin{1};
    else
        K = length(varargin);
        input = varargin;
    end
    
    % cell array for the gradients of all images
    G = cell(K,1);
    % cell array for the pairwise scalar products of the gradients
    S = cell(K, K);
    % cell array for the norms of the gradients
    N = cell(K, 1);   
    
    for k = 1 : K        
        % calulate the gradient
        G{k} = Gradient(input{k}, 'symmetric', 'central', 1, 0);
        % and store its norm
        N{k} = Norm_3(G{k}, 0);
    end
    
    % For all pairs of images compute their scalar product of the
    % gradients. As the scalar product is symmetric it is sufficient to 
    % skip almost half of the pairs.
    for u = 1 : K
        for v=1:K
            if u ~= v
                if u < v
                    S{u, v} = Inner_prod_3(G{u},G{v});
                else
                    S{u, v} = S{v, u};
                end
            end
        end;
    end   
                
    f = @(Nu, Nv, uv, sigma)(Norm_3(Nu, beta).*Norm_3(Nv, beta) - Norm_3(uv, beta^2));
    
    fun_image = zeros(size(input{1}));
    c = numel(input{1});
    
    if nargout > 1
        grad = zeros(c* K,1);
    end

    [Kappa, Tau] = Diffusivities(beta, G, N, S);
    
    for u=1:K
        if nargout > 1
            I = (u-1) * c + (1 : c);
            grad(I) = - Diffusion(Kappa{u}, input{u});
        end
        
        for v = 1 : K
            m = 1;
            if nargout > 1 && u ~= v
                grad(I) = grad(I) - Diffusion(Tau{m}, input{v});
                m = m + 1;
            end
            
            if v > u                
                fun_image = fun_image + f(N{u}, N{v}, S{u,v}, beta);
            end
        end
    end
    
    fun = sum(fun_image(:));
end

function [kappa, tau] = Diffusivities(beta, G, N, S)
% [[kappa, tau] = Diffusivities(beta, G, N, S)
% 
% Diffusivities computes the diffusivites for functional for the gradient
% of the Parallel Level Sets functional.
%
% 2013-07-26, Matthias J. Ehrhardt, CMIC, University College London, UK

    K = length(G);
    KK = K*(K-1)/2.;
    
    % output cell arrays for kappa and tau
    kappa = cell(K, 1);
    tau = cell(KK, 1);

    % compute kappa and tau.
    f_kappa = @(Nx, Ny, xy, beta)(Norm_3(Ny, beta) ./ Norm_3(Nx, beta));
    f_tau   = @(Nx, Ny, xy, beta)(- xy ./ Norm_3(xy, beta^2));
    
    k = 1;
    for u=1:K
        kappa{u} = zeros(size(N{1}));
        
        for v=1:K
            if u ~= v
                kappa{u} = kappa{u} + f_kappa(N{u},N{v},S{u,v},beta);
                if u > v
                    tau{k} = f_tau(N{u},N{v},S{u,v},beta);
                    k = k + 1;
                end
            end
        end
    end              
end